#include "wxCamViewWindow.h"

#include "../Camera.h"
#include <cv.h>
#include <cvaux.h>
#include "MainFrame.h"
#include "../nxColor.h"
#include "../Configuration/RobotIdentification.h"
#include "../Configuration/RobotProject.h"
#include "../Configuration/Robot.h"
#include "DialogPoint.h"
 
wxCamViewWindow::wxCamViewWindow( wxWindow *frame, const wxPoint& pos, const wxSize& size ):
			wxWindow(frame, -1, pos, size, wxSIMPLE_BORDER )
{
	this->Connect( wxEVT_LEFT_DOWN, wxMouseEventHandler( wxCamViewWindow::OnLeftDown ) );
	this->Connect( wxEVT_LEFT_UP, wxMouseEventHandler( wxCamViewWindow::OnLeftUp ) );
	this->Connect( wxEVT_MOTION, wxMouseEventHandler( wxCamViewWindow::OnMove ) );
	this->Connect( wxEVT_PAINT,wxPaintEventHandler( wxCamViewWindow::OnPaint ) );
	this->Connect( wxEVT_SIZE,wxSizeEventHandler( wxCamViewWindow::OnSize ) );
	 
	isAxesEnabled=false;   
	selectEnable=false;
	selecting=false;
	filterOn = false;
	parent = (MainFrame*) this->GetParent();
	
	EtalonParams = new double[3];
	EtalonParams[0] = 8;
	EtalonParams[1] = 8;
	EtalonParams[2] = 30.0;
	calibFilesPath = new char[200];
	calibFilesPath =  strcat(get_current_dir_name(),"/");///home/alejo/Escritorio/CalibrationDemoOpenCV/img/";
	
	//calibFilter.SetEtalon ( CV_CALIB_ETALON_CHESSBOARD, EtalonParams );
	//calibFilter.SetCameraCount ( 1 );
	
	mCalibFilter = new CvCalibFilter();
	isCalibEnabled = false;
	proyectPlaneReady = false;
	cvImg = NULL;
}

 wxCamViewWindow::~wxCamViewWindow()
 {
 	delete mCalibFilter;
 	}
 


void wxCamViewWindow::OnPaint(wxPaintEvent& event )
{
	wxPaintDC dc(this);
	Draw( dc );
}

void wxCamViewWindow::OnSize(wxSizeEvent& event )
{
	int windowWidth; 
	int windowHeight;
	this->GetSize(&windowWidth,&windowHeight);
	wxImg.SetWidth(windowWidth);
	wxImg.SetHeight(windowHeight);
	Refresh( FALSE );
	Update( );
}


void wxCamViewWindow::Draw(wxPaintDC &dc)
{
	int x,y,w,h;
	dc.GetClippingBox( &x, &y, &w, &h );
	dc.DrawBitmap( wxImg, x, y );
		
}

 
void wxCamViewWindow::updateImg()
{

	if(cvImg)
	{
		
		
		int windowWidth; 
		int windowHeight;
		
		this->GetSize(&windowWidth,&windowHeight);
		
		//IplImage ** img = &cvImg;
		IplImage **images=new IplImage*[1];
		images[0] = cvImg;
		/*if(isCalibEnabled)
			mCalibFilter->Undistort ( images, images);
		*/
		
			
		//this stage is executed if the process button has being pulsed. It will identify the color blobs
		//in the image.
		if(filterOn && parent->getColorNumber()>0 )
		{
			
			//get the array of configurated colors
			MainFrame* parent = (MainFrame*) this->GetParent();
			nxColor** listColor = parent->getColorList();
			int colorNum = parent->getColorNumber();
			
			nxColor* color;
			
			
			
			
			//this is used to stablish wheter the user wants to see all colors or just one at a time
			//TODO: not yet implemented, it only shows all the colors.
			if(parent->getSelectedClass()!=-1)
				color = listColor[parent->getSelectedClass()-1];
			else
				color = listColor[0];
			
			
			RobotProject* rp = parent->getRobotProject();
			RobotIdentification* rid =new RobotIdentification(rp->getRobotArray(),rp->getNumRobots(), mVision);
			
			//conversion de la imagen a HSV
			IplImage* hsvImg =cvCreateImage(cvGetSize(cvImg),IPL_DEPTH_8U,3);
			cvCvtColor(cvImg,hsvImg,CV_RGB2HSV);

			mVision->processFrame((image_pixel*)hsvImg->imageData);

			cvReleaseImage(&hsvImg);
			
			//draw blobs in image
			
			
			identifyColors(mVision, cvImg, colorNum,parent->getMinArea(),parent->getMaxArea(),isCalibEnabled);
			
			//draw robot centers in image
			identifyRobots(cvImg,rid, proyectPlaneReady,isCalibEnabled);
			
    		
			
		}
				
		//show axes
		if(isAxesEnabled)
		{
			//font for numbers of blobs
			CvFont laFont;
			cvInitFont(&laFont,CV_FONT_HERSHEY_SIMPLEX,0.3f,0.3f);
			
			//get image size
			int cheight = camera->getCamHeight();
			int cwidth = camera->getCamWidth();
			
			int a = 4;
			//TODO: falta cambiar para el tamaño de la imagen, esta fija la resolucion de 640x480
			cvLine(cvImg,cvPoint(cwidth/2 -1,0),cvPoint(cwidth/2 -1,cheight-1),cvScalar(26,255,0));
			cvLine(cvImg,cvPoint(0,cheight/2-1),cvPoint(cwidth -1,cheight/2-1),cvScalar(26,255,0));
			cvPolyLine( cvImg, &imagePoints, &a,1, 1, CV_RGB(0,0,255), 1, CV_AA, 0);
			
			char* text = new char[20];
			
			for(int i=0;i<4;i++)
			{
				
				sprintf(text,"(%4.2f,%4.2f)",realPoints[i].x,realPoints[i].y);
				cvPutText(cvImg,text,imagePoints[i],&laFont,cvScalarAll(255));
			}
			
		}
		
		//convert image to wxwidgets format
		wxImg = cvImg2wxBitmap(cvImg,(int)windowWidth,(int)windowHeight);
		
		Refresh( FALSE );
	
		Update( );	
		
	}
}


 
void wxCamViewWindow::resetImg()
{
	if(cvImg)
	{
		cvResetImageROI(cvImg);
		updateImg();
	}
}

bool wxCamViewWindow::isProyectPlaneReady()
{
	return proyectPlaneReady;
}

void wxCamViewWindow::getCamImage()
{

	if(this->camera && this->camera->isCamOn())
	{
		double fps = calculateFps();
		if(fps>0)
		{
			// Print frame rate.
			char value[30];
			wxString wxValue;
			
			sprintf(value, "Framerate: %f fps", fps);
			wxValue = wxValue.FromAscii(value);
			
			parent->setStatusBarText(wxValue);
		}
		
		
		cvImg =this->camera->queryImage();
		
		//if an actual image vas received
		if(cvImg)
		{
			cvResetImageROI(cvImg);
			IplImage* RGBImg=cvImg;// = cvCloneImage(cvImgTemp);
			
			cvCvtColor(cvImg,RGBImg,CV_BGR2RGB);
			//cvImg=RGBImg;
			updateImg();
		}
		//cvReleaseImage(&cvImgTemp);
	}

}

void wxCamViewWindow::initCam(int camID, char *camSrc,int camWidth,int camHeight)
{
	camera->camInit(camID, camSrc,camWidth,camHeight);
	CMvisionInitialize(camera->getCamWidth(),camera->getCamHeight());
}

void wxCamViewWindow::setCam(Camera* camera)
{
	this->camera = camera;

}

void wxCamViewWindow::releaseCam()
{
	//delete camera;
	camera->cameraRelease();
	wxImg=NULL;
	Refresh( FALSE );

	Update( );
}

void wxCamViewWindow::OnLeftDown(wxMouseEvent& event )
{
	
	//TODO: Todavia no se puede señalar puntos de perspectiva si no se han cargado los parametros de calibracion
	
	static char allPointsDefined=0;
	if(selectEnable)
	{
		initRectX = event.GetX();
		initRectY = event.GetY();
		wxPaintDC dc(this);
		
		selecting=true;
	}
	else if(isAxesEnabled)
	{
		
		CvRect ROIRect = cvGetImageROI(cvImg);
		
		//conversion constants, used to convert coordinates of the wxPaintDC to the image
		float CONV_WIDTH =  (float)ROIRect.width / (float)wxImg.GetWidth();
		float CONV_HEIGHT = (float)ROIRect.height / (float)wxImg.GetHeight();
		
		int windowWidth, windowHeight;
		
		this->GetSize(&windowWidth,&windowHeight);
		
		if(event.GetX()<windowWidth/2)
			//sector 1
			if(event.GetY()<windowHeight/2)
			{  
				float x;
				float y;
				DialogPoint* a = new DialogPoint(this->GetParent(),&x,&y);
				
				realPoints[0] = cvPoint2D32f(x,y);
				
				imagePoints[0] = cvPoint((int)(event.GetX()*CONV_WIDTH),(int)(event.GetY()*CONV_HEIGHT));
				
				//if params of camera already loaded, correct lens dist
				//if(getIntrinsicParams()!=NULL)
					//correctLensDist(imagePoints[0],&imagePoints[0]);
				
				//mark the spot on a register to know that point on sector1 was selected
				allPointsDefined |= (1<<0);
				
				
			}
			//sector 4
			else
			{
				float x;
				float y;
				DialogPoint* a = new DialogPoint(this->GetParent(),&x,&y);
				realPoints[3] = cvPoint2D32f(x,y);
				imagePoints[3] = cvPoint((int)(event.GetX()*CONV_WIDTH),(int)(event.GetY()*CONV_HEIGHT));
				
				//if params of camera already loaded, correct lens dist
				//if(getIntrinsicParams()!=NULL)
					//correctLensDist(imagePoints[3],&imagePoints[3]);
					
				//mark the spot on a register to know that point on sector1 was selected
				allPointsDefined |= (1<<3);
			}
			else
			{
				//sector 2
				if(event.GetY()<windowHeight/2)
				{
					float x;
					float y;
					DialogPoint* a = new DialogPoint(this->GetParent(),&x,&y);
					realPoints[1] = cvPoint2D32f(x,y);
					imagePoints[1] = cvPoint((int)(event.GetX()*CONV_WIDTH),(int)(event.GetY()*CONV_HEIGHT));
					
					//if params of camera already loaded, correct lens dist
					//if(getIntrinsicParams()!=NULL)
						//correctLensDist(imagePoints[1],&imagePoints[1]);
					//mark the spot on a register to know that point on sector1 was selected
					allPointsDefined |= (1<<1);
				}
				//sector 3
				else
				{
					float x;
					float y;
					DialogPoint* a = new DialogPoint(this->GetParent(),&x,&y);
					realPoints[2] = cvPoint2D32f(x,y);
					imagePoints[2] = cvPoint((int)(event.GetX()*CONV_WIDTH),(int)(event.GetY()*CONV_HEIGHT));
					
					//TODO: CORREGIR ESTO PORQ NO SE PUEDE ESCOGER ENTRE VER EL MARCO SIN DIST Y CON DIST
					//if params of camera already loaded, correct lens dist
					//if(getIntrinsicParams()!=NULL)
						//correctLensDist(imagePoints[2],&imagePoints[2]);
					//mark the spot on a register to know that point on sector1 was selected
					allPointsDefined |= (1<<2);
				}
				
			}
			
		if(allPointsDefined == 0x0F)
		{
			proyectPlaneReady = true;
			getProjectionMatrix();
		}	
		
		
	}
	
	
}

void wxCamViewWindow::OnMove(wxMouseEvent& event )
{
	if(selectEnable && selecting)
	{
		finalRectX = event.GetX();
		finalRectY = event.GetY();
		
		wxPaintDC dc(this);
		Draw(dc);
		dc.SetBrush(*wxTRANSPARENT_BRUSH);
		
		wxPen* pen = new wxPen(*wxWHITE, 1, wxSHORT_DASH);
		dc.SetPen(*pen);
		dc.DrawRectangle(initRectX,initRectY,finalRectX-initRectX,finalRectY-initRectY);
	}

}
void wxCamViewWindow::OnLeftUp(wxMouseEvent& event )
{
	if(selectEnable)
	{
		selecting=false;
		if(event.GetX()>initRectX && event.GetY()>initRectY){
			finalRectX = event.GetX();
			finalRectY = event.GetY();
		}
		else
		{
			if(event.GetX()>initRectX && event.GetY()<initRectY){
				finalRectY = initRectY;
				finalRectX = event.GetX();
				initRectY = event.GetY();			
			}
			else{
				if(event.GetX()<initRectX && event.GetY()<initRectY){
					finalRectY = initRectY;
					finalRectX = initRectX;
					initRectY = event.GetY();
					initRectX = event.GetX();				
				}
				else{
					if(event.GetX()<initRectX && event.GetY()>initRectY){
						finalRectX = initRectX;
						initRectX = event.GetX();
						finalRectY = event.GetY();
											
					}
				}
			}
		}
		
	
		CvRect ROIRect = cvGetImageROI(cvImg);
		float CONV_WIDTH =  (float)ROIRect.width / (float)wxImg.GetWidth();
		float CONV_HEIGHT = (float)ROIRect.height / (float)wxImg.GetHeight();
		
		//Size of window
		int windowWidth; 
		int windowHeight;
			
		this->GetSize(&windowWidth,&windowHeight);
		

		wxImg = cvImg2wxBitmap(selectImgArea(cvImg,cvPoint((int)(initRectX*CONV_WIDTH),(int)(initRectY*CONV_HEIGHT)),cvPoint((int)(finalRectX*CONV_WIDTH),(int)(finalRectY*CONV_HEIGHT))),windowWidth,windowHeight);
		
		if(wxImg.IsSameAs(wxImage()))
			setSelectEnable(false);	
		
		Refresh( FALSE );
		
		Update( );
	}
}

bool wxCamViewWindow::isSelectEnable()
{
	return selectEnable;
}

void wxCamViewWindow::setSelectEnable(bool selectEnable)
{
	this->selectEnable = selectEnable;
}


bool wxCamViewWindow::isFilterOn()
{
	return filterOn;
}

void wxCamViewWindow::setFilterOn(bool state)
{
	this->filterOn = state;
}


IplImage* wxCamViewWindow::getCvImage()
{
	return cvImg;	
}



void wxCamViewWindow::traceAxes(bool state)
{
	isAxesEnabled=state;
}

